#include "frskyTx.h"

#ifdef __linux
#include <iostream>     // cout
#include <stdio.h>      // write

using namespace std;
#endif

frskyTx::frskyTx () {
    //ctor
    int i;
    this->lipoCells = 0;
    for (i=0; i<12; i++) this->lipoVolt[i] = 0;
}

frskyTx::~frskyTx () {
    //dtor
}

unsigned char frskyTx::frame1[] = {
        0x5e, 0x24, 0x00, 0x00,   // AccX
        0x5e, 0x25, 0x00, 0x00,   // AccY
        0x5e, 0x26, 0x00, 0xfe,   // AccZ
        0x5e, 0x10, 0x00, 0x00,   // Altitude (before .)
        0x5e, 0x21, 0x00, 0x00,   // Altitude (after .)
        0x5e, 0x02, 0x00, 0x00,   // Temp1
        0x5e, 0x05, 0x00, 0x00,   // Temp2
        0x5e, 0x06, 0x00, 0x00,   // Lipo volt
        0x5e, 0x28, 0x00, 0x00,   // Amp amp
        0x5e, 0x3a, 0x00, 0x00,   // Amp volt (before .)
        0x5e, 0x3b, 0x00, 0x00,   // Amp volt (after .)
        0x5e, 0x03, 0x00, 0x00,   // RPM
        0x5e
};

unsigned char frskyTx::frame2[] = {
        0x5e, 0x14, 0x2c, 0x00,   // GPS course (before .)
        0x5e, 0x1c, 0x03, 0x00,   // GPS course (after .)
        0x5e, 0x13, 0x38, 0x0c,   // GPS latitude (before .)
        0x5e, 0x1b, 0xc9, 0x06,   // GPS latitude (after .)
        0x5e, 0x23, 0x4e, 0x00,   // N/S
        0x5e, 0x12, 0xef, 0x2e,   // GPS longitude (before .)
        0x5e, 0x1a, 0x98, 0x26,   // GPS longitude (after .)
        0x5e, 0x22, 0x45, 0x00,   // E/W
        0x5e, 0x11, 0x02, 0x00,   // GPS speed (before .)
        0x5e, 0x19, 0x93, 0x00,   // GPS speed (after .)
        0x5e, 0x01, 0x18, 0x00,   // GPS altitude (before .)
        0x5e, 0x09, 0x05, 0x00,   // GPS altitude (after .)
        0x5e, 0x04, 0x00, 0x00,   // Fuel level
        0x5e
};

unsigned char frskyTx::frame3[] = {
        0x5e, 0x15, 0x0f, 0x07,   // Day - month
        0x5e, 0x16, 0x0b, 0x00,   // Year
        0x5e, 0x17, 0x06, 0x12,   // Hours - minutes
        0x5e, 0x18, 0x32, 0x00,   // Seconds
        0x5e
};

float frskyTx::getLipoVoltSum () {
    float r = 0.0;
    int i;
    for (i = 0; i < lipoCells; i++) {
        r += lipoVolt[i];
    }
    return r;
}

void frskyTx::setAccX (float a) {
    // frame1 - little endian - signed
    int c = a * 1000.0;
    frame1[2] = c;
    frame1[3] = c >> 8;
}

void frskyTx::setAccY (float a) {
    // frame1 - little endian - signed
    int c = a * 1000.0;
    frame1[6] = c;
    frame1[7] = c >> 8;
}

void frskyTx::setAccZ (float a) {
    int c = a * 1000.0;
    frame1[10] = c;
    frame1[11] = c >> 8;
}

void frskyTx::setAltitude (float l) {
    // frame1 - little endian - unsigned
    unsigned int b, a;
    b = (int) l;
    a = (int) ((l-b) * 10.0);
    frame1[14] = b;
    frame1[15] = b >> 8;
    frame1[18] = a;
}

void frskyTx::setAmpAmp (int a) {
    // frame1 - little endian - unsigned
    frame1[34] = a;
    frame1[35] = a >> 8;
}

void frskyTx::setAmpVolt (float v) {
    // frame1 - little endian - unsigned
    unsigned int b, a;
    b = (int) v;
    a = (int) ((v-b) * 10.0);
    frame1[38] = b;
    frame1[39] = b >> 8;
    frame1[42] = a;
}

void frskyTx::setDayMonthYear (unsigned int d, unsigned int m, unsigned int y) {
    // frame3 - unused and untested
    frame3[2] = d;
    frame3[3] = m;
    frame3[6] = y;
    frame3[7] = y >> 8;
}

void frskyTx::setGpsAltitude (float l) {
    // frame2 - unused and untested
    signed int a, b;
    b = (int) l;
    a = (int) ((l-b) * 100);
    frame2[42] = b;
    frame2[43] = b >> 8;
    frame2[46] = a;
}

void frskyTx::setGpsSpeedKmh (float s) {
    // frame2 - little endian - unsigned
    // buggy display: 100 kmph / 1.852 -> 99.7 kmph
    float kn = s / 1.852; // km to knots conversion
    unsigned int b, a;
    b = (int) kn;
    a = (int) ((kn-b) * 100.0);
    frame2[34] = b;
    frame2[35] = b >> 8;
    frame2[38] = a;
}

void frskyTx::setFuel (unsigned int f) {
    // frame2 - little endian - unsigned
    // valid values: 0, 25, 50, 75, 100
    if (f > 100) f = 100;
    int a = f / 25.0 + 0.5; // round 1/25
    frame2[50] = a * 25;
}

void frskyTx::setHourMinSec (unsigned int h, unsigned int m, unsigned int s) {
    frame3[10] = h;
    frame3[11] = m;
    frame3[14] = s;
}

void frskyTx::setLipo (int c, float v) {
    // frame1 - big endian - unsigned
    // one cell per frame1
    int val = v / 4.2 * 2100.0;
    frame1[30] = c << 4 | val >> 8;
    frame1[31] = val;
    if (c+1 > lipoCells) lipoCells = c+1;
    lipoVolt[c] = v;
}

void frskyTx::setRpm (unsigned int r) {
    // frame1 - little endian - unsigned
    // Quite bad precision (increments of 40 / 2 blades)
    r = r / 60;
    frame1[46] = r;
    frame1[47] = r >> 8;
}

void frskyTx::setTemp1 (signed t) {
    // frame1 - little endian - signed
    frame1[22] = t;
    frame1[23] = t >> 8;
}

void frskyTx::setTemp2 (signed t) {
    // frame1 - little endian - signed
    frame1[26] = t;
    frame1[27] = t >> 8;
}

void frskyTx::sendFrame1 (int fd) {
    write (fd, frame1, 49);
    int i;
    for (i=0; i < 49; i++) {
        printf ("%02x ", frame1[i]);
    }
    cout << endl;
}

void frskyTx::sendFrame2 (int fd) {
    write (fd, frame2, 53);
    int i;
    for (i=0; i < 53; i++) {
        printf ("%02x ", frame2[i]);
    }
    cout << endl;
}

void frskyTx::sendFrame3 (int fd) {
    write (fd, frame3, 17);
    int i;
    for (i=0; i < 17; i++) {
        printf ("%02x ", frame3[i]);
    }
    cout << endl;
}
